#include <particle_filter.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>

int main(int argc, char** argv)
{
    std::vector<double> xs;
    std::vector<double> ys;
    std::vector<double> ps;
    //std::vector<double> realxs;
    //std::vector<double> realys;
    //double start_x = 0 , start_y = 0;
    std::ofstream myfile_ps;
    myfile_ps.open ("ps_data.txt");

    std::ofstream myfile_xs;
    myfile_xs.open ("xs_data.txt");

    std::ofstream myfile_ys;
    myfile_ys.open ("ys_data.txt");

    Particle_Filter filter(1000, 4,0,0,1.0,1);
    //Particle_Filter filter(1000, 0,0,1.0,0,1);
    /*

    STRAIGHT LINE

    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.update_particle_weight(5.0,0,1.0,0);
    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.move_particles(1);
    filter.printParticlesPositions(xs, ys);
    filter.update_particle_weight(10.0,0,1.0,0);
    filter.printParticlesPositions(xs, ys);
    */

    // equation x^2 + y^2 = 4
    //

    float angle =0;
    float cx = 0;
    float cy = 0;
    float cvx =0;
    float cvy =0;
    float ct = 0;

    float d_angle = 0.01;
    float d_time = 0.2;
    float speed = 4.0 * d_angle / d_time;

    float mx, my;


    //filter.printParticlesPositions(xs, ys,ps);

    for(int i = 0; i<= 628; i++)
    {
        angle = d_angle * i;
        cx = cos(angle)*4;
        cy = sin(angle)*4;
        ct = 0.2 * i;

        std::cout << cvx << " " << cvy <<std::endl;

        cvx = speed *-sin(angle);
        cvy = speed * cos(angle);

        filter.move_particles(0.05);
        filter.update_particle_weight(cx,cy,cvx,cvy);
        //filter.printParticlesPositions(xs,ys,ps);
        filter.mean_position(mx,my);
        xs.push_back(mx);
        ys.push_back(my);
    }

//    for(int i = 0; i<= 628; i++)
//    {
//        cx = i * 0.1;
//        cy = 0;
//        ct = 0.1 * i;

//        filter.move_particles(0.1);
//        filter.update_particle_weight(cx,cy,1,0);
//        filter.mean_position(mx,my);
//        xs.push_back(mx);
//        ys.push_back(my);
//    }

    for(int i = 0; i < xs.size(); ++i)
    {
        std::stringstream s;
        s << xs[i] << "\n";
        myfile_xs << s.str();
    }

    for(int i = 0; i < ps.size(); ++i)
    {
        std::stringstream s;
        s << ps[i] << "\n";
        myfile_ps << s.str();
    }

    for(int i = 0; i < ys.size(); ++i)
    {
        std::stringstream s;
        s << ys[i] << "\n";
        myfile_ys << s.str();
    }

    myfile_xs.close();
    myfile_ys.close();
    myfile_ps.close();


    system("python plotparticles.py");
    return 0;
}
